//
// Created by fins on 24-9-28.
//

#ifndef CLOUD_FILTER_CLOUD_FILTER_HPP
#define CLOUD_FILTER_CLOUD_FILTER_HPP

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>
#include <string>
#include "livox_ros_driver2/msg/custom_point.hpp"
#include "livox_ros_driver2/msg/custom_msg.hpp"
#include "std_msgs/msg/header.hpp"
#include <vector>
#include "cube.hpp"

/***
 * CustomPoint: Custom pointcloud data structure
 * CustomMsg: Custom pointcloud message structure
 */
struct CustomPoint {
    uint32_t offset_time;    // Offset time relative to the base time
    float x;                 // X axis, unit: m
    float y;                 // Y axis, unit: m
    float z;                 // Z axis, unit: m
    uint8_t reflectivity;    // Reflectivity, 0~255
    uint8_t tag;             // Livox tag
    uint8_t line;            // Laser number in lidar
    CustomPoint& operator=(const CustomPoint& other) = default;
};
struct CustomMsg {
    std_msgs::msg::Header header;  // ROS standard message header
    uint64_t timebase;              // The time of first point
    uint32_t point_num;             // Total number of pointclouds
    uint8_t lidar_id;               // Lidar device id number
    std::array<uint8_t, 3> rsvd;    // Reserved use
    std::vector<CustomPoint> points; // Pointcloud data
    CustomMsg& operator=(const CustomMsg& other) = default;
};
// CustomMsgPtr: Shared pointer of CustomMsg
using CustomMsgPtr = std::shared_ptr<CustomMsg>;

/***
    * @brief Convert CustomPoint to ROS message
    * @param custom_point CustomPoint data structure
    * @return livox_ros_driver2::msg::CustomPoint_<std::allocator<void>> ROS message
    */
CustomMsg fromRosMsg(const livox_ros_driver2::msg::CustomMsg_<std::allocator<void>>& ros_msg);

/***
    * @brief Convert ROS message to CustomPoint
    * @param custom_msg ROS message
    * @return CustomPoint data structure
    */
livox_ros_driver2::msg::CustomMsg_<std::allocator<void>> toRosMsg(const CustomMsg& custom_msg);

/***
 * CloudFilter: Filter pointcloud data
 * @function filter: Filter pointcloud data @param input_cloud: Input pointcloud data @param output_cloud: Output pointcloud data
 * @function loadParams: Load parameters from yaml file @param yaml_file: Yaml file path
 */
class CloudFilter {
public:
    CloudFilter();
    void filter(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
                pcl::PointCloud<pcl::PointXYZ>::Ptr& output_cloud);
    void set_transform_r2l(const geometry_msgs::msg::TransformStamped& transform);
    void loadParams(const std::string& yaml_file);

private:
    Eigen::Vector3f box_min_;
    Eigen::Vector3f box_max_;
    std::string transform_to_robot_frame;
    geometry_msgs::msg::TransformStamped transform_r2l;
    Cube cube_;
};
/***
 * CloudFilter_Custom: Filter custom pointcloud data
 * @function filter: Filter custom pointcloud data @param input_cloud: Input custom pointcloud data @param output_cloud: Output custom pointcloud data
 * @function loadParams: Load parameters from yaml file @param yaml_file: Yaml file path
 */
class CloudFilter_Custom {
public:
    CloudFilter_Custom();
    void filter(const CustomMsgPtr& input_cloud,
                CustomMsgPtr& output_cloud);
    void set_transform_r2l(const geometry_msgs::msg::TransformStamped& transform);
    void loadParams(const std::string& yaml_file);

private:
    Eigen::Vector3f box_min_;
    Eigen::Vector3f box_max_;
    std::string transform_to_robot_frame;
    geometry_msgs::msg::TransformStamped transform_r2l;
    Cube cube_;
};






#endif // CLOUD_FILTER_CLOUD_FILTER_HPP
